package behavior;

import arena.Node;
import graphics.Point;
import java.util.ArrayList;
import java.util.Collections;
import java.util.ConcurrentModificationException;
import java.util.List;
import robot.Robot;

/**
 * PSOBehavior is a smart swarm based algorithm and is adapted from the
 * standard Particle Swarm Optimization algorithm for use with robots.  Each
 * robot is treated as a particle in space with a direction.  Based
 * on euclidean distance the algorithm determines which robot is closest to
 * the goal location.  Once a robot has been identified all other robots
 * change their direction towards the the one closest to the goal.  Robots
 * are only given their distance from the goal and not the goal location itself.
 * Because of the limited information that is given to the robots the PSO
 * behavior simulates inter-robot communication using a software mailbox
 * facilitated by a synchronized List.
 *
 * @author      Gabriel Loewen
 */
public class PSOBehavior extends Behavior {

    /**
     * The synchronized list used as a software mailbox between robots.
     */
    public static final List<String[]> messageList = Collections.synchronizedList(new ArrayList());
    /**
     * The synchronized list of robots in the swarm
     */
    public static List<Robot> robotList = Collections.synchronizedList(new ArrayList());
    /**
     * The distance between the local best node and the goal
     */
    private double localDist;
    /**
     * The distance between the global best node and the goal
     */
    private double globalDist;
    /**
     * Reference to the local best node
     */
    private Node localBestNode;
    /**
     * Reference to the gloval best node
     */
    private Node globalBestNode;
    /**
     * Reference to the current node that the robot is on
     */
    private Node currentNode;
    /**
     * Reference to the goal node
     */
    private Node targetNode;
    /**
     * Boolean value representing the existence of the goal.  Used for error checking
     */
    private boolean targetExists = false;
    /**
     * Velocity components (x and y) for the determination of the next move
     */
    private int[] velocity;
    /**
     * Boolean used as a lock for the synchronized mailbox
     */
    public static boolean messageAccess = false;

    /**
     * Default Constructor
     */
    public PSOBehavior() {
        super("Particle Swarm Optimization");
    }

    /**
     * Each robot that is assigned to the PSO behavior is added to
     * a synchronized list of robots.  This is used to pass messages to each
     * robot in the swarm.
     *
     * @param   r   The robot being added to the list
     *
     */
    @Override
    public void setRobot(Robot r) {
        super.setRobot(r);
        robotList.add(robot);
    }

    /**
     * Returns the euclidean distance between 2 nodes.
     *
     * @param   x   The starting node
     * @param   y   The ending node
     *
     */
    protected double getEuclidean(Node x, Node y) {
        return Math.abs(Math.sqrt(Math.pow(y.getLocation().x - x.getLocation().x, 2) + Math.pow(y.getLocation().y - x.getLocation().y, 2)));
    }

    /**
     * Returns the distance between 2 nodes as an array with 2 elements.
     * Example:
     *
     *      int[] comp = getComponents(node1, node2);
     *
     *      comp[0] ====> The difference between nodes in the x direction.
     *      comp[1] ====> The difference between nodes in the y direction.
     *
     * @param   x   The starting node
     * @param   y   The ending node
     *
     */
    protected int[] getComponents(Node x, Node y) {
        return new int[]{y.getLocation().x - x.getLocation().x, y.getLocation().y - x.getLocation().y};
    }


    /**
     * Announces the current euclidean distance for the robot cooresponding to
     * this behavior into the synchronized message list and determines if the
     * current position of the robot is the closest that this robot has been.
     * If this location is the closest location to the goal then it updates
     * this robots local best node to the current node.  This information is used
     * to help narrow in on the goal node.
     */
    protected void announce() {
        double euclidean = getEuclidean(currentNode, targetNode);
        messageList.add(new String[]{String.valueOf(euclidean), currentNode.getLocation().toString(), "0"});
        if (localDist > euclidean) {
            localDist = euclidean;
            localBestNode = currentNode;
        }
    }

    /**
     * This will read all of the messages in the synchronized message list
     * in order to determine the closest node that any of the robots have
     * found thus far.  This node is determined to be the global best node.
     */
    protected void updateGlobalBest() {
        double euclidean;
        String[] message;
        int newIndex;
        synchronized (messageList) {
            try {
                while (messageAccess) {
                    try {
                        messageList.wait();
                    } catch (Exception e) {
                        messageList.notifyAll();
                    }
                }
                messageAccess = true;
                for (int i = 0; i < messageList.size(); i++) {
                    message = messageList.get(i);

                    euclidean = Double.valueOf(message[0]);
                    if (euclidean < globalDist) {
                        globalDist = euclidean;
                        globalBestNode = robot.getArena().nodeMap.get(message[1]);
                    }
                    newIndex = Integer.valueOf(message[2]) + 1;
                    if (newIndex == robotList.size()) {
                        messageList.remove(i);
                    } else {
                        messageList.get(i)[2] = String.valueOf(newIndex);
                    }
                }
            } catch (ConcurrentModificationException ex) {
            }
            messageAccess = false;
            messageList.notifyAll();
        }
    }

    /**
     * Announces the current euclidean distance for the robot cooresponding to
     * this behavior into the synchronized message list and calculates the
     * next movement that the robot should make by determining a pseudo
     * velocity using an average of the components of the distance between the
     * local best node and the global best node.  In addition, the average
     * distance is scaled by a random factor before being accumulated.  This
     * randomization helps to allow the robot to move in the best direction but
     * still allows for the possibility of their being an even better direction.
     * After the velocty in the x direction and the y direction is calculated
     * the magnitude of each component is determined (absolute value) and
     * the next node is determined by whichever has a larger magnitude.
     *
     * @return A reference to a point object which is the next position in (X,Y) format
     */
    protected Point getFitness() {

        announce();
        while (messageList.size() < robotList.size()) {
        }
        updateGlobalBest();

        int[] globalComponents = getComponents(currentNode, globalBestNode);
        int[] localComponents = getComponents(currentNode, localBestNode);

        velocity[0] += (int) Math.floor(globalComponents[0] * Math.random() + 0.5) + (int) Math.floor(localComponents[0] * Math.random() + 0.5);
        velocity[1] += (int) Math.floor(globalComponents[1] * Math.random() + 0.5) + (int) Math.floor(localComponents[1] * Math.random() + 0.5);

        //System.out.println("(" + robot.robotID + ") VX = " + velocity[0] + " VY = " + velocity[1]);

        Point nextPoint = new Point(robot.getLocation().x, robot.getLocation().y);

        int xSign = (velocity[0] > 0) ? 1 : -1;
        int ySign = (velocity[1] > 0) ? 1 : -1;
        if (Math.abs(velocity[0]) > Math.abs(velocity[1])) {
            nextPoint.x += xSign;
        } else if (Math.abs(velocity[1]) > Math.abs(velocity[0])) {
            nextPoint.y += ySign;
        }

        if (!nodeExists(nextPoint.toString()) || robot.getArena().nodeMap.get(nextPoint.toString()).isOccupied) {
            nextPoint.x = robot.getLocation().x;
            nextPoint.y = robot.getLocation().y;
            List adj = Node.getAdjNodes(robot.getArena().nodeMap.get(nextPoint.toString()));
            Node nextNode = null;
            int i = 0;
            while (i < 100) {
                nextNode = (Node) adj.get((int) Math.floor(Math.random() * (adj.size() - 1) + 0.5));
                if (nodeExists(nextNode.toString()) && !nextNode.isOccupied) {
                    break;
                }
                i++;
            }
            if (nextNode != null && nodeExists(nextNode.toString()) && !nextNode.isOccupied) {
                nextPoint = nextNode.getLocation();
            }
        }

        return nextPoint;

    }

    /**
     * Because the algorithm determines the next move all moves are valid.
     * @return Always true.  The algorithm determines validity automatically
     */
    public boolean isValidMove() {
        return true;
    }

    /**
     * Initializes the variables necessary for calculation of velocity
     * and local and global best nodes.
     *
     * @param   goal      The goal location in (X,Y) format
     * @param   cancel    Not necessary for this behavior.
     */
    public void getPath(String goal, boolean cancel) {
        if (nodeExists(goal) && !cancel) {
            globalBestNode = null;
            localBestNode = null;
            localDist = Double.MAX_VALUE;
            globalDist = Double.MAX_VALUE;
            velocity = new int[2];
            targetNode = robot.getArena().nodeMap.get(goal);
            if (targetNode != null) {
                targetExists = true;
            }
        }
    }

    /**
     * Returns true if the goal node is unoccupied.  As soon as one of the
     * robots in the swarm finds the goal all robots cease moving.
     * @return True if the target node exists and is not occupied
     */
    @Override
    public boolean hasMove() {
        currentNode = robot.getArena().nodeMap.get(robot.getLocation().toString());
        if (!targetExists || targetNode.isOccupied) {
            messageList.clear();
            return false;
        } else {
            return true;
        }
    }

    /**
     * Returns an integer array containing the data needed for the
     * next move in the queue.  The next move is determined by
     * calling the getFitness method.
     *
     * @see Util
     * @return A standard int array composed of heading, location and new direction
     */
    @Override
    public int[] nextMove() {
        char dir = robot.heading;
        Point nextPoint = getFitness();
        Node nextNode = robot.getArena().nodeMap.get(nextPoint.toString());
        if (nextNode != null && !nextNode.isOccupied) {
            if (nextPoint.x > robot.getLocation().x) {
                dir = 'E';
            } else if (nextPoint.x < robot.getLocation().x) {
                dir = 'W';
            } else {
                if (nextPoint.y > robot.getLocation().y) {
                    dir = 'N';
                } else if (nextPoint.y < robot.getLocation().y) {
                    dir = 'S';
                }
            }
            return Util.getMoveData(robot.heading, robot.getLocation().x, robot.getLocation().y, dir);
        }
        return new int[]{'X'};
    }

    @Override
    public void finalize() throws Throwable {
        robotList.remove(robot);
        super.finalize();
    }

    /**
     * Always returns 100 because this behavior is not path oriented
     * @return 100
     */
    @Override
    public double getProgress() {
        return 100;
    }

    /**
     * Unimplemented because there is no path implementation
     */
    @Override
    public void resetProgress() {
    }
}
